#include <iostream>
#include <random>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>


// 计算两个平面的交线方程（两点法）
pcl::ModelCoefficients computeIntersection(const pcl::ModelCoefficients& plane1, const pcl::ModelCoefficients& plane2, pcl::PointXYZ& intersection_point1, pcl::PointXYZ& intersection_point2) {
    pcl::ModelCoefficients intersection;
    Eigen::Vector3f normal1(plane1.values[0], plane1.values[1], plane1.values[2]);
    Eigen::Vector3f normal2(plane2.values[0], plane2.values[1], plane2.values[2]);

    // 计算两个法向量的夹角
    float angle = pcl::getAngle3D(normal1, normal2);

    // 如果夹角大于30度，则计算交线
    if (angle > 30.0f * M_PI / 180.0f) {
        // 选择其中一个平面的距离原点的距离作为参数（例如，使用 D1）

        Eigen::Vector3f direction = normal1.cross(normal2); // 交线方向为两个法向量的叉积
        intersection.values.resize(6);
        intersection.values[0] = direction[0];
        intersection.values[1] = direction[1];
        intersection.values[2] = direction[2];

        Eigen::Matrix2d A;
        A << plane1.values[0], plane1.values[1], plane2.values[0], plane2.values[1];

        // 定义右侧向量 b
        Eigen::Vector2d b;
        b << plane1.values[3], plane2.values[3];

        // 定义解向量 x
        Eigen::Vector2d x;

        // 使用Eigen的线性方程求解器求解方程组 Ax = b
        x = A.inverse() * b;
        
        intersection.values[3] = x[0];
        intersection.values[4] = x[1];
        intersection.values[5] = 0;

        // 计算交点1
        intersection_point1.x = intersection.values[3]+x[0];
        intersection_point1.y = intersection.values[4]+x[1];
        intersection_point1.z = intersection.values[5];

        // 计算交点2
        intersection_point2.x = x[0];
        intersection_point2.y = x[1];
        intersection_point2.z = 0;

        


    } else {
        intersection.values.resize(0); // 夹角小于30度，表示无效的交线
        intersection_point1.x = intersection_point1.y = intersection_point1.z = std::numeric_limits<float>::quiet_NaN();
        intersection_point2.x = intersection_point2.y = intersection_point2.z = std::numeric_limits<float>::quiet_NaN();
    }

    return intersection;
}




int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("./scan3.pcd", *cloud) == -1)
    {   
        PCL_ERROR("Couldn't read file.\n");
        return (-1);
    }
    pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");

    // 创建一个随机数生成器引擎
    std::random_device rd;  // 随机设备，用于生成随机种子
    std::mt19937 gen(rd()); // Mersenne Twister 伪随机数生成器
    // 定义随机数的分布范围
    std::uniform_int_distribution<int> distribution(0, 255); // 生成1到100之间的随机整数

    // 生成随机数
    //int random_number = ;

    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(3.8); // 设置距离阈值，用于确定点是否属于平面

    // 创建容器来存储分割结果的点和系数
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

    std::vector<pcl::ModelCoefficients> coefficients_vector;



    int plane_index = 0;

    while (cloud->points.size() > 1000)
    {
        seg.setInputCloud(cloud);
        seg.segment(*inliers, *coefficients);
        coefficients_vector.push_back(*coefficients);
        if (inliers->indices.size() == 0)
        {
            std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        // 使用提取索引滤波器提取属于平面的点
        pcl::ExtractIndices<pcl::PointXYZRGB> extract;
        extract.setInputCloud(cloud);
        extract.setIndices(inliers);
        extract.setNegative(false); // 设置为true以提取非平面点，设置为false以提取平面点

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGB>);
        extract.filter(*plane);

        // 为每个平面分配一个唯一的颜色
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color_handler(plane, distribution(gen), distribution(gen), distribution(gen)); // 设置RGB颜色值，这里为红色

        // 将渲染的平面添加到可视化窗口
        std::string plane_id = "plane_" + std::to_string(plane_index);
        viewer.addPointCloud(plane, color_handler, plane_id);
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, plane_id);
        std::cout<<plane_id<<std::endl;

        // 移除已经分割的平面点
        extract.setNegative(true);
        extract.filter(*cloud);

        plane_index++;
    }

    for (size_t i = 0; i < coefficients_vector.size(); ++i) {
        for (size_t j = i + 1; j < coefficients_vector.size(); ++j) {
            // 计算两个平面的交线和交点
            pcl::PointXYZ intersection_point1, intersection_point2;
            pcl::ModelCoefficients intersection = computeIntersection(coefficients_vector[i], coefficients_vector[j], intersection_point1, intersection_point2);

            // 检查是否有效的交线
            if (intersection.values.size() > 0) {
                // 添加交线到可视化窗口
                viewer.addLine(intersection_point1, intersection_point2, "line_" + std::to_string(i) + "_" + std::to_string(j));
                viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "line_" + std::to_string(i) + "_" + std::to_string(j));
            }
        }
    }

    viewer.setBackgroundColor(0, 0, 0);

    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }

    return 0;
}